//------------------------------------------------------------------------------
// Test Bench Service
//
// Professional Microsoft Robotics Developer Studio
//
// Updated for RDS 2008 (V2.0)
//
//------------------------------------------------------------------------------
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;

using System;
using System.Collections.Generic;

using Microsoft.Robotics.Simulation;
using Microsoft.Robotics.Simulation.Engine;
using engineproxy = Microsoft.Robotics.Simulation.Engine.Proxy;
using Microsoft.Robotics.Simulation.Physics;
using Microsoft.Robotics.PhysicalModel;
using System.ComponentModel;

using xna = Microsoft.Xna.Framework;

namespace ProMRDS.Simulation.TestBench
{
    [DisplayName("TestBench")]
    [Description("Joint Experimentation TestBench")]
    [Contract(Contract.Identifier)]
    public class TestBenchService : DsspServiceBase
    {
        [Partner("Engine",
            Contract = engineproxy.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        private engineproxy.SimulationEnginePort _engineStub =
            new engineproxy.SimulationEnginePort();

        // Main service port
        [ServicePort("/SimulationTutorial1", AllowMultipleInstances = false)]
        private TestBenchOperations _mainPort =
            new TestBenchOperations();

        public TestBenchService(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {

        }

        protected override void Start()
        {
            base.Start();
            // Orient sim camera view point
            SetupCamera();
            // Add objects (entities) in our simulated world
            PopulateWorld();
        }

        private void SetupCamera()
        {
            // Set up initial view
            CameraView view = new CameraView();
            view.EyePosition = new Vector3(-26.03f, 1.17f, -0.51f);
            view.LookAtPoint = new Vector3(-25.06f, 0.92f, -0.4f);
            SimulationEngine.GlobalInstancePort.Update(view);
        }

        private void PopulateWorld()
        {
            AddSky();
            AddGround();

            Vector3 position = new Vector3(0.5f - 25, 0, 0);
            string name;
            SingleShapeSegmentEntity segment;

            // add a parent entity box
            BoxShape bench = new BoxShape(new BoxShapeProperties(
                10000, new Pose(), new Vector3(50, 0.5f, 1)));
            bench.State.DiffuseColor = new Vector4(0.8f, 0.8f, 0.8f, 1);
            SingleShapeEntity benchEntity = new SingleShapeEntity(bench, new Vector3(0, 0.25f, 0));
            benchEntity.State.Name = "TestBenchEntity";
            benchEntity.State.Pose.Orientation = new Quaternion(0, 0, 0, 1);
            SimulationEngine.GlobalInstancePort.Insert(benchEntity);

            // Add several joints and associated cameras
            JointAngularProperties angular;
            JointLinearProperties linear;
            EntityJointConnector[] connectors;

            // Simple single DOF joint
            name = "1DOF";
            segment = NewSegment(position + new Vector3(0, 0.5f, 0), name);
            angular = new JointAngularProperties();
            angular.TwistMode = JointDOFMode.Free;
            angular.TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(500000, 100000, 0), 100000000);
            connectors = new EntityJointConnector[2];
            connectors[0] = new EntityJointConnector(
                segment,
                new Vector3(0,1,0),
                new Vector3(1,0,0),
                new Vector3(0, -segment.CapsuleShape.State.Dimensions.Y/2 - segment.CapsuleShape.State.Radius, 0));

            connectors[1] = new EntityJointConnector(
                benchEntity,
                new Vector3(0,1,0),
                new Vector3(1,0,0),
                new Vector3(position.X, position.Y + 0.25f, position.Z));

            segment.CustomJoint = new Joint();
            segment.CustomJoint.State = new JointProperties(angular, connectors);
            segment.CustomJoint.State.Name = name + "-twist";
            benchEntity.InsertEntityGlobal(segment);
            AddCamera(position, name);
            position.X += 2;

            // A joint with a very small damper coefficient on the drive
            name = "SmallDamper";
            segment = NewSegment(position + new Vector3(0, 0.5f, 0), name);
            angular = new JointAngularProperties();
            angular.TwistMode = JointDOFMode.Free;
            angular.TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 100, 0), 100000000);
            connectors = new EntityJointConnector[2];
            connectors[0] = new EntityJointConnector(
                segment,
                new Vector3(0, 1, 0),
                new Vector3(1, 0, 0),
                new Vector3(0, -segment.CapsuleShape.State.Dimensions.Y / 2 - segment.CapsuleShape.State.Radius, 0));

            connectors[1] = new EntityJointConnector(
                benchEntity,
                new Vector3(0, 1, 0),
                new Vector3(1, 0, 0),
                new Vector3(position.X, position.Y + 0.25f, position.Z));

            segment.CustomJoint = new Joint();
            segment.CustomJoint.State = new JointProperties(angular, connectors);
            segment.CustomJoint.State.Name = name + "-twist";
            benchEntity.InsertEntityGlobal(segment);
            AddCamera(position, name);
            position.X += 2;

            // A joint with all 6 DOF free
            name = "6DOF";
            segment = NewSegment(position + new Vector3(0, 0.5f, 0), name);
            angular = new JointAngularProperties();
            angular.TwistMode = JointDOFMode.Free;
            angular.Swing1Mode = JointDOFMode.Free;
            angular.Swing2Mode = JointDOFMode.Free;
            angular.TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 100, 0), 100000000);
            angular.SwingDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 100, 0), 100000000);

            linear = new JointLinearProperties();
            linear.XMotionMode = JointDOFMode.Free;
            linear.XDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 100, 0), 100000000);
            linear.YMotionMode = JointDOFMode.Free;
            linear.YDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 100, 0), 100000000);
            linear.ZMotionMode = JointDOFMode.Free;
            linear.ZDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(50000000, 100, 0), 100000000);
            connectors = new EntityJointConnector[2];
            connectors[0] = new EntityJointConnector(
                segment,
                new Vector3(0, 1, 0),
                new Vector3(1, 0, 0),
                new Vector3(0, -segment.CapsuleShape.State.Dimensions.Y / 2 - segment.CapsuleShape.State.Radius, 0));

            connectors[1] = new EntityJointConnector(
                benchEntity,
                new Vector3(0, 1, 0),
                new Vector3(1, 0, 0),
                new Vector3(position.X, position.Y + 0.25f, position.Z));

            segment.CustomJoint = new Joint();
            segment.CustomJoint.State = new JointProperties(angular, connectors);
            segment.CustomJoint.State.Linear = linear;
            segment.CustomJoint.State.Name = 
                name + "-twist" + ";" +
                name + "-swing1" + ";" +
                name + "-swing2" + ";" +
                name + "-X|-2|2|" + ";" +
                name + "-Y|-2|2|" + ";" +
                name + "-Z|-2|2|" + ";" ;
            benchEntity.InsertEntityGlobal(segment);
            AddCamera(position, name);
            position.X += 2;

            // Simple single DOF joint with differing connector orientations
            name = "Orientation";
            segment = NewSegment(position + new Vector3(0, 0.5f, 0), name);
            angular = new JointAngularProperties();
            angular.Swing1Mode = JointDOFMode.Free;
            angular.SwingDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(500000, 100, 0), 100000000);
            connectors = new EntityJointConnector[2];
            connectors[0] = new EntityJointConnector(
                segment,
                new Vector3(0, 0, 1),
                new Vector3(0, 1, 0),
                new Vector3(0, -segment.CapsuleShape.State.Dimensions.Y / 2 - segment.CapsuleShape.State.Radius, 0));

            connectors[1] = new EntityJointConnector(
                benchEntity,
                new Vector3(0, 1, 0),
                new Vector3(1, 0, 0),
                new Vector3(position.X, position.Y + 0.25f, position.Z));

            segment.CustomJoint = new Joint();
            segment.CustomJoint.State = new JointProperties(angular, connectors);
            segment.CustomJoint.State.Name = name + "-swing1";
            benchEntity.InsertEntityGlobal(segment);
            AddCamera(position, name);
            position.X += 2;

            // An angular joint with limited twist and swing2
            name = "Limited";
            segment = NewSegment(position + new Vector3(0, 0.5f, 0), name);
            angular = new JointAngularProperties();
            angular.TwistMode = JointDOFMode.Limited;
            angular.Swing2Mode = JointDOFMode.Limited;
            angular.TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(5000, 100, 0), 100000000);
            angular.SwingDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(5000, 100, 0), 100000000);
            
            // specify a limit on twist and swing2 with an angle of PI/8, 0 restitution, and no spring
            angular.LowerTwistLimit = new JointLimitProperties((float)(-Math.PI / 8), 0, new SpringProperties(5000000, 1000, 0));
            angular.UpperTwistLimit = new JointLimitProperties((float)(Math.PI / 8), 0, new SpringProperties(5000000, 1000, 0));
            angular.Swing2Limit = new JointLimitProperties((float)(Math.PI / 8), 0, new SpringProperties(5000000, 1000, 0));

            connectors = new EntityJointConnector[2];
            connectors[0] = new EntityJointConnector(
                segment,
                new Vector3(0, 1, 0),
                new Vector3(1, 0, 0),
                new Vector3(0, -segment.CapsuleShape.State.Dimensions.Y / 2 - segment.CapsuleShape.State.Radius, 0));

            connectors[1] = new EntityJointConnector(
                benchEntity,
                new Vector3(0, 1, 0),
                new Vector3(1, 0, 0),
                new Vector3(position.X, position.Y + 0.25f, position.Z));

            segment.CustomJoint = new Joint();
            segment.CustomJoint.State = new JointProperties(angular, connectors);

            segment.CustomJoint.State.Name =
                name + "-twist" + ";" +
                name + "-swing2" + ";";
            benchEntity.InsertEntityGlobal(segment);
            AddCamera(position, name);
            position.X += 2;
        }

        SingleShapeSegmentEntity NewSegment(Vector3 position, string name)
        {
            CapsuleShape segmentShape = new CapsuleShape(new CapsuleShapeProperties(
                10, new Pose(), 0.08f, 0.5f));

            segmentShape.State.DiffuseColor = new Vector4(0.5f, 0.5f, 0.8f, 1);
            SingleShapeSegmentEntity se = new SingleShapeSegmentEntity(segmentShape, position + new Vector3(0, segmentShape.State.Dimensions.Y/2 + segmentShape.State.Radius, 0));
            se.State.Name = name + "_segment";
            se.State.Pose.Orientation = new Quaternion(0, 0, 0, 1);
            return se;
        }

        void AddCamera(Vector3 position, string name)
        {
            CameraEntity cam = new CameraEntity();
            cam.Location = TypeConversion.ToXNA(position) + new xna.Vector3(0, 2, 2);
            cam.LookAt = TypeConversion.ToXNA(position) + new xna.Vector3(0, 1, 0);
            cam.State.Name = name + "_cam";
            SimulationEngine.GlobalInstancePort.Insert(cam);
        }

        void AddSky()
        {
            // Add a sky using a static texture. We will use the sky texture
            // to do per pixel lighting on each simulation visual entity
            SkyDomeEntity sky = new SkyDomeEntity("skydome.dds", "sky_diff.dds");
            SimulationEngine.GlobalInstancePort.Insert(sky);

            // Add a directional light to simulate the sun.
            LightSourceEntity sun = new LightSourceEntity();
            sun.State.Name = "Sun";
            sun.Type = LightSourceEntityType.Directional;
            sun.Color = new Vector4(0.8f, 0.8f, 0.8f, 1);
            sun.Direction = new Vector3(0.5f, -.75f, 0.5f);
            SimulationEngine.GlobalInstancePort.Insert(sun);
        }

        void AddGround()
        {
            // create a large horizontal plane, at zero elevation.
            HeightFieldEntity ground = new HeightFieldEntity(
                "simple ground", // name
                "floor.bmp", // texture image
                new MaterialProperties("ground",
                    0.2f, // restitution
                    0.5f, // dynamic friction
                    0.5f) // static friction
                );
            SimulationEngine.GlobalInstancePort.Insert(ground);
        }
    }

    public static class Contract
    {
        public const string Identifier = "http://www.promrds.com/contracts/2007/09/testbench.html";
    }

    [ServicePort]
    public class TestBenchOperations : PortSet<DsspDefaultLookup, DsspDefaultDrop>
    {
    }
    
    /// <summary>
    /// Defines a new entity type that overrides the ParentJoint with 
    /// custom joint properties.  It also handles serialization and
    /// deserialization properly.
    /// </summary>
    [DataContract]
    public class SingleShapeSegmentEntity : SingleShapeEntity
    {
        private Joint _customJoint;

        [DataMember]
        public Joint CustomJoint
        {
            get { return _customJoint; }
            set { _customJoint = value; }
        }

        /// <summary>
        /// Default constructor
        /// </summary>
        public SingleShapeSegmentEntity() { }

        /// <summary>
        /// Initialization constructor
        /// </summary>
        /// <param name="shape"></param>
        /// <param name="initialPos"></param>
        public SingleShapeSegmentEntity(Shape shape, Vector3 initialPos)
            : base(shape, initialPos)
        {
        }

        public override void Initialize(Microsoft.Xna.Framework.Graphics.GraphicsDevice device, PhysicsEngine physicsEngine)
        {
            base.Initialize(device, physicsEngine);

            // update the parent joint to match our custom joint parameters
            if (_customJoint != null)
            {
                if(ParentJoint != null)
                    PhysicsEngine.DeleteJoint((PhysicsJoint)ParentJoint);

                // restore the entity pointers in _customJoint after deserialization if necessary
                if (_customJoint.State.Connectors[0].Entity == null)
                    _customJoint.State.Connectors[0].Entity = FindConnectedEntity(_customJoint.State.Connectors[0].EntityName, this);

                if (_customJoint.State.Connectors[1].Entity == null)
                    _customJoint.State.Connectors[1].Entity = FindConnectedEntity(_customJoint.State.Connectors[1].EntityName, this);

                ParentJoint = _customJoint;
                PhysicsEngine.InsertJoint((PhysicsJoint)ParentJoint);
            }
        }

        VisualEntity FindConnectedEntity(string name, VisualEntity me)
        {
            // find the parent at the top of the hierarchy
            while (me.Parent != null)
                me = me.Parent;

            // now traverse the hierarchy looking for the name
            return FindConnectedEntityHelper(name, me);
        }

        VisualEntity FindConnectedEntityHelper(string name, VisualEntity me)
        {
            if (me.State.Name == name)
                return me;

            foreach (VisualEntity child in me.Children)
            {
                VisualEntity result = FindConnectedEntityHelper(name, child);
                if (result != null)
                    return result;
            }

            return null;
        }

        /// <summary>
        /// Override the base PreSerialize method so that we can properly serialize joints
        /// </summary>
        public override void PreSerialize()
        {
            base.PreSerialize();
            PrepareJointsForSerialization();
        }
    }
}
